/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package us.oh.k12.wkw.robot;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.networktables.NetworkTable;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import java.util.Date;
import us.oh.k12.wkw.robot.command.AutonomousBehindToShootBack;
import us.oh.k12.wkw.robot.command.AutonomousBehindToShootLeft;
import us.oh.k12.wkw.robot.command.AutonomousBehindToShootRight;
import us.oh.k12.wkw.robot.command.AutonomousTurnCmd;
import us.oh.k12.wkw.robot.command.CommandBase;
import us.oh.k12.wkw.robot.command.DriveInSquareCmd;
import us.oh.k12.wkw.robot.command.DriveStraightCmd;
import us.oh.k12.wkw.robot.util.SimpleDateFormat;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {

    private static final String VERSION = "3.0.2"; // program version
    private static final int DELAY_COUNT_START = 20;
    private static final int DELAY_DISABLED_START = 200;
    private int delayCounter = 0;
    private Command autonomousCommand;
    //private boolean autonomousMode = false;
    //private boolean teleopMode = false;
    //private boolean testMode = false;
    private SendableChooser autoChooser;

    public String getName() {
        return "Robot";
    }

    public String getSmartDashboardType() {
        return "Robot";
    }

    public Robot() {
        super();
    }

    /**
     * Robot-wide initialization code should go here. Robot-wide initialization
     * which will be called when the robot is first powered on. It will be
     * called exactly 1 time.
     */
    public void robotInit() {

        try {

            // Initialize all subsystems
            CommandBase.init();

            // instantiate the command used for the autonomous period

            this.autoChooser = new SendableChooser();
            this.autoChooser.addDefault("DriveStraightCmd", new DriveStraightCmd(2));
            this.autoChooser.addObject("DriveInSquareCmd", new DriveInSquareCmd());
            this.autoChooser.addObject("AutonmousTurnCmd", new AutonomousTurnCmd());
            this.autoChooser.addObject("AutonomousBehindToShootRight", new AutonomousBehindToShootRight());
            this.autoChooser.addObject("AutonomousBehindToShootLeft", new AutonomousBehindToShootLeft());
            this.autoChooser.addObject("AutonomousBehindToShootBack", new AutonomousBehindToShootBack());

            SmartDashboard.putData("AutoChooser", this.autoChooser);

            this.autonomousCommand = new DriveStraightCmd(2);

            //
            // initialize the smart dashboard data
            //

            this.debug("robotInit()", "Robot initialized.");

        } catch (Exception anEx) {
            // catch all exceptions so we can gracefully log
            // and figure out what happened
            // instead of the robot just crashing
            this.error("robotInit()", anEx);
        }

    }

    /*
     *
     * disabled mode methods.
     */
    /**
     * Initialization code for disabled mode should go here - initialization
     * code which will be called each time the robot enters disabled mode.
     */
    public void disabledInit() {

        try {

            this.autonomousCommand.cancel();

            this.flushStatus();

            this.delayCounter = Robot.DELAY_COUNT_START;

            this.debug("disabledInit()", "Called.");

        } catch (Exception anEx) {
            this.error("disabledInit()", anEx);
        }
    }

    /**
     * Periodic code for disabled mode should go here - will be called
     * periodically at a regular rate while the robot is in disabled mode.
     */
    public void disabledPeriodic() {

        try {

            if (this.delayCounter-- < 0) {
                // we don't need to publish often during disabled.
                this.delayCounter = Robot.DELAY_DISABLED_START;
                // publish data to the dashboard
                this.updateStatus();
            }

        } catch (Exception anEx) {
            this.error("disabledPeriodic()", anEx);
        }
    }

    /*
     *
     * autonomous mode methods.
     */
    public void autonomousInit() {

        try {

            this.autonomousCommand = (Command) this.autoChooser.getSelected();

            // schedule the autonomous command
            this.autonomousCommand.start();

            // publish data to the dashboard
            this.flushStatus();

            this.debug("autonomousInit()", "Called.");

        } catch (Exception anEx) {
            this.error("autonomousInit()", anEx);
        }
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {

        try {

            // publish data to the dashboard
            if (this.delayCounter-- < 0) {
                // we don't need to publish often during disabled.
                this.delayCounter = Robot.DELAY_COUNT_START;
                // publish data to the dashboard
                this.updateStatus();
            }

            // run the command scheduler
            // has five stages:
            // 1. Poll the buttons
            // 2. Execute/Remove the Commands.
            // 3. Send values to SmartDashboard.
            // 4. Add Commands.
            // 5. Add Defaults.
            // TODO: why gets nullpointerexception
            try {

                Scheduler.getInstance().run();

            } catch (NullPointerException aNpEx) {
                this.error("autonomousPeriodic()", null);
            }

        } catch (Exception anEx) {
            this.error("autonomousPeriodic()", anEx);
        }
    }

    /*
     *
     * teleop mode methods.
     */
    public void teleopInit() {

        try {

            // This makes sure that the autonomous stops running when
            // teleop starts running. If you want the autonomous to
            // continue until interrupted by another command, remove
            // this line or comment it out.
            this.autonomousCommand.cancel();

            // publish data to the dashboard
            this.flushStatus();

            this.debug("teleopInit()", "Called.");

        } catch (Exception anEx) {
            this.error("teleopInit()", anEx);
        }
    }

    /**
     * This function is called periodically during telelop (operator) control
     */
    public void teleopPeriodic() {

        try {

            // this.debug("teleopPeriodic()", "Called.");

            // publish data to the dashboard
            if (this.delayCounter-- < 0) {
                // we don't need to publish often during disabled.
                this.delayCounter = Robot.DELAY_COUNT_START;
                // publish data to the dashboard
                this.updateStatus();
            }

            // run the command scheduler
            // has five stages:
            // 1. Poll the buttons
            // 2. Execute/Remove the Commands.
            // 3. Send values to SmartDashboard.
            // 4. Add Commands.
            // 5. Add Defaults.
            // TODO: why gets nullpointerexception
            try {

                Scheduler.getInstance().run();

            } catch (NullPointerException aNpEx) {
                this.error("teleopPeriodic()", "Scheduler error.", null);
            }

        } catch (Exception anEx) {
            this.error("teleopPeriodic()", "Caught " + anEx.getClass().getName() + ", message="
                    + anEx.getMessage() + ".", anEx);
        }
    }

    public void testInit() {

        try {


            this.autonomousCommand.cancel();

            // publish data to the dashboard
            this.flushStatus();

            this.debug("testInit()", "Called.");

        } catch (Exception anEx) {
            this.error("testInit()", anEx);
        }
    }

    public void testPeriodic() {

        try {

            // this.debug("testPeriodic()", "Called.");

            LiveWindow.run();

        } catch (Exception anEx) {
            this.error("testPeriodic()", anEx);
        }
    }

    /**
     * Get the robot program version.
     *
     * @return version.
     */
    public static String getVersion() {
        return Robot.VERSION;
    }

    /*
     *
     * support methods.
     */
    private void flushStatus() {

        CommandBase.updateStatus();
        // have all subsystems flush all state to the dashboard
        CommandBase.flushStatus();

    }

    protected void updateStatus() {

        // have all subsystems publish data to the dashboard
        CommandBase.updateStatus();

    }

    protected void debug(final String pMethod, final String pMessage) {

        NetworkTable.getTable(Robot.NETWORK_TABLE_NAME).putString(
                Robot.NETWORK_TABLE_FIELD_NAME, this.formatMessage(new Date(), this.getClassName(), "D", pMethod, pMessage));

        System.out.println(pMethod + " " + pMessage);
    }

    protected void info(final String pMethod, final String pMessage) {

        NetworkTable.getTable(Robot.NETWORK_TABLE_NAME).putString(
                Robot.NETWORK_TABLE_FIELD_NAME, this.formatMessage(new Date(), this.getClassName(), "I", pMethod, pMessage));

        System.out.println(pMethod + " " + pMessage);
    }

    private void error(final String pMethod, final Exception pEx) {
        this.error(pMethod, this.formatException(pEx), pEx);
    }

    private void error(final String pMethod, final String pMessage, final Exception pEx) {

        NetworkTable.getTable(Robot.NETWORK_TABLE_NAME).putString(
                Robot.NETWORK_TABLE_FIELD_NAME, this.formatMessage(new Date(), this.getClassName(), "E", pMethod, pMessage));


        System.out.println(pMethod + " " + pMessage);

        if (null != pEx) {
            pEx.printStackTrace();
        }
    }

    private String formatException(final Exception pEx) {
        return (null == pEx ? "." : pEx.getClass().getName() + ", message=" + pEx.getMessage()
                + ".");
    }

    protected String formatMessage(final Date pMessageDate, final String pClazz,
            final String pLevel, final String pMethod, final String pMessage) {

        final StringBuffer aMsg = new StringBuffer();

        aMsg.append(Robot.formatDate(pMessageDate));
        aMsg.append(Robot.SEPERATOR);
        aMsg.append(pLevel);
        aMsg.append(Robot.SEPERATOR);
        aMsg.append(pClazz);
        aMsg.append(Robot.SEPERATOR);
        aMsg.append(pMethod);
        aMsg.append(Robot.SEPERATOR);
        aMsg.append(pMessage);

        return aMsg.toString();
    }
    private static final char SEPERATOR = ',';
    private static final String FORMAT = "yyyy/mm/dd_HH:mm:ss.sss";
    private static final String NETWORK_TABLE_NAME = "WkwFrcLogger";
    private static final String NETWORK_TABLE_FIELD_NAME = "log";

    protected static String formatDate(final Date pDate) {
        final SimpleDateFormat aFormat = new SimpleDateFormat(Robot.FORMAT);
        return (null == pDate ? "" : aFormat.format(pDate));
    }

    protected String getDateTimeFormatted() {
        final SimpleDateFormat aformat = new SimpleDateFormat();
        return aformat.format(new Date());
    }

    private String getClassName() {
        String aClassName = this.getClass().getName();
        return aClassName.substring(aClassName.lastIndexOf('.') + 1);
    }
}
